/**
  ******************************************************************************
  * Copyright (C) 2020 - ~, SCUT-RobotLab Development Team
  * @file    state_StandUp.cpp
  * @author  Mentos Seetoo
  * @brief   Finite State Machine for standing up.
  * @note    
  * @warning 
  *     - At least C++11 is required.														 
  ******************************************************************************
  */
#include "state_Passive.h"

/* Private defines -----------------------------------------------------------*/
using namespace Quadruped;

/* Founctions ----------------------------------------------------------------*/
template <typename T>
FSM_Passive<T>::FSM_Passive(GaitScheduler<T> *p_gait,
                            StateEstimator<T> *p_estimator,
                            DesiredCoMState<T> *p_command,
                            Dynamic<T> *p_dynamic,
                            T run_period):FSM_Base<T>::FSM_Base(run_period)
{
    FSM_Base<T>::_name = FSM_StateName::PASSIVE;
    p_Dynamic = p_dynamic;
    p_GaitScheduler = p_gait;
    p_CoMCommand = p_command;
    p_StateEstimator = p_estimator;
    _cnt = 0;
}


template <typename T>
void FSM_Passive<T>::RunState()
{
    //!< Set and stay in the init position.
    for(int leg(0); leg < 4; leg++)
    {
        if(p_StateEstimator->p_initFoot[leg](2)==0)
            p_StateEstimator->p_initFoot[leg] = p_Dynamic->LegState[leg].p
                                              + p_Dynamic->getHipLocation(leg);

        p_Dynamic->LegCommand[leg].p = p_StateEstimator->p_initFoot[leg] 
                                        - p_Dynamic->getHipLocation(leg);
    }

    //!< After a constant time start to stand up.
    if(++_cnt > 5)
        FSM_Base<T>::SetNextState(FSM_StateName::STAND_UP);
}

/**
* @brief  Passive state can only switch to STAND_UP
* @return always be true, reentry is no need.
* @author Mentos Seetoo
*/
template <typename T>
int FSM_Passive<T>::Transition(FSM_Base<T>* target_state)
{
    switch (target_state->StateName())
    {
        case FSM_StateName::PASSIVE:
            break;
        case FSM_StateName::STAND_UP:
            SysLog->Record(_INFO_,"FSM","State is switched to STAND_UP...");
            break;
        default:
            SysLog->Record(_ERROR_,"FSM","Error transition in PASSIVE, request is ignored...");
            break;
    }

    return 0;
}

template class FSM_Passive<float>;
template class FSM_Passive<double>;
/************************ COPYRIGHT(C) SCUT-ROBOTLAB **************************/